#ifndef __ROB_SENSOR_H__
#define __ROB_SENSOR_H__

#include "rob_common.h"
#include "mfl.h"  /* Include EASML for filtering functions */

/* Sensor Type Definitions */
typedef enum {
    ROB_SENSOR_ULTRASONIC = 0u,
    ROB_SENSOR_INFRARED = 1u,
    ROB_SENSOR_TOUCH = 2u,
    ROB_SENSOR_COLOR = 3u,
    ROB_SENSOR_SOUND = 4u,
    ROB_SENSOR_GYRO = 5u,
    ROB_SENSOR_ACCEL = 6u
} Rob_SensorType_Type;

/* Ultrasonic Sensor Structure */
typedef struct {
    uint16 TrigPin_u16;        /* Trigger pin */
    uint16 EchoPin_u16;        /* Echo pin */
    float32 Distance_f32;      /* Measured distance (mm) */
    float32 MaxRange_f32;      /* Maximum range (mm) */
    uint32 LastMeasTime_u32;   /* Last measurement time */
    Rob_Status_Type Status;    /* Sensor status */
} Rob_UltrasonicSensor_Type;

/* IR Sensor Structure */
typedef struct {
    uint16 Pin_u16;            /* Analog input pin */
    uint16 RawValue_u16;       /* Raw ADC value */
    float32 Distance_f32;      /* Calculated distance (mm) */
    float32 Voltage_f32;       /* Input voltage (V) */
    Rob_Status_Type Status;    /* Sensor status */
} Rob_InfraredSensor_Type;

/* Touch Sensor Structure */
typedef struct {
    uint16 Pin_u16;            /* Digital input pin */
    boolean IsPressed_b;       /* Current state */
    boolean LastState_b;       /* Previous state */
    uint32 PressTime_u32;      /* Press duration (ms) */
    uint32 LastChangeTime_u32; /* Last state change time */
    Rob_Status_Type Status;    /* Sensor status */
} Rob_TouchSensor_Type;

/* Color Sensor Structure (TCS34725 compatible) */
typedef struct {
    uint8 I2cAddress_u8;       /* I2C device address */
    uint16 RedValue_u16;       /* Red channel value */
    uint16 GreenValue_u16;     /* Green channel value */
    uint16 BlueValue_u16;      /* Blue channel value */
    uint16 ClearValue_u16;     /* Clear channel value */
    float32 Lux_f32;           /* Calculated illuminance */
    uint16 ColorTemp_u16;      /* Color temperature */
    Rob_Status_Type Status;    /* Sensor status */
} Rob_ColorSensor_Type;

/* Sound Sensor Structure */
typedef struct {
    uint16 Pin_u16;            /* Analog input pin */
    uint16 RawValue_u16;       /* Raw ADC value */
    float32 SoundLevel_f32;    /* Sound level (dB) */
    float32 Threshold_f32;     /* Detection threshold */
    boolean IsTriggered_b;     /* Trigger status */
    Rob_Status_Type Status;    /* Sensor status */
} Rob_SoundSensor_Type;

/* IMU Sensor Structure */
typedef struct {
    uint8 I2cAddress_u8;       /* I2C device address */
    float32 AccelX_f32;        /* X-axis acceleration (g) */
    float32 AccelY_f32;        /* Y-axis acceleration (g) */
    float32 AccelZ_f32;        /* Z-axis acceleration (g) */
    float32 GyroX_f32;         /* X-axis angular velocity (deg/s) */
    float32 GyroY_f32;         /* Y-axis angular velocity (deg/s) */
    float32 GyroZ_f32;         /* Z-axis angular velocity (deg/s) */
    float32 Roll_f32;          /* Roll angle (degrees) */
    float32 Pitch_f32;         /* Pitch angle (degrees) */
    float32 Yaw_f32;           /* Yaw angle (degrees) */
    Rob_Status_Type Status;    /* Sensor status */
} Rob_ImuSensor_Type;

/* Sensor Filtering Structure (using EASML) */
typedef struct {
    Mfl_StatePT1_Type FilterState;  /* EASML PT1 filter state */
    float32 FilterTc_f32;           /* Time constant */
    float32 SampleTime_f32;         /* Sample time */
    boolean Enable_b;               /* Filter enable flag */
} Rob_SensorFilter_Type;

/* Sensor System Structure */
typedef struct {
    Rob_UltrasonicSensor_Type Ultrasonic;
    Rob_InfraredSensor_Type Infrared;
    Rob_TouchSensor_Type Touch;
    Rob_ColorSensor_Type Color;
    Rob_SoundSensor_Type Sound;
    Rob_ImuSensor_Type Imu;
    Rob_SensorFilter_Type Filters[ROB_SENSOR_NUM];
    uint32 LastUpdateTime_u32;
    Rob_Status_Type SystemStatus;
} Rob_SensorSystem_Type;

/* Sensor Event Structure */
typedef struct {
    Rob_SensorType_Type SensorType;
    uint32 Timestamp_u32;
    float32 Value_f32;
    boolean IsTriggered_b;
} Rob_SensorEvent_Type;

/* Function Declarations */

/* System Initialization */
Rob_Status_Type Rob_SensorInit(Rob_SensorSystem_Type *System_pst);

/* Ultrasonic Sensor Functions */
Rob_Status_Type Rob_UltrasonicInit(Rob_UltrasonicSensor_Type *Sensor_pst,
                                  uint16 TrigPin_u16, uint16 EchoPin_u16);

Rob_Status_Type Rob_UltrasonicRead(Rob_UltrasonicSensor_Type *Sensor_pst);

float32 Rob_UltrasonicGetDistance_f32(const Rob_UltrasonicSensor_Type *Sensor_cpst);

/* Infrared Sensor Functions */
Rob_Status_Type Rob_InfraredInit(Rob_InfraredSensor_Type *Sensor_pst,
                                uint16 Pin_u16);

Rob_Status_Type Rob_InfraredRead(Rob_InfraredSensor_Type *Sensor_pst,
                                uint16 AdcValue_u16);

float32 Rob_InfraredGetDistance_f32(const Rob_InfraredSensor_Type *Sensor_cpst);

/* Touch Sensor Functions */
Rob_Status_Type Rob_TouchInit(Rob_TouchSensor_Type *Sensor_pst, uint16 Pin_u16);

Rob_Status_Type Rob_TouchUpdate(Rob_TouchSensor_Type *Sensor_pst,
                               boolean CurrentState_b, uint32 CurrentTime_u32);

boolean Rob_TouchIsPressed_b(const Rob_TouchSensor_Type *Sensor_cpst);

boolean Rob_TouchIsReleased_b(const Rob_TouchSensor_Type *Sensor_cpst);

/* Color Sensor Functions */
Rob_Status_Type Rob_ColorInit(Rob_ColorSensor_Type *Sensor_pst,
                             uint8 I2cAddress_u8);

Rob_Status_Type Rob_ColorRead(Rob_ColorSensor_Type *Sensor_pst);

Rob_Status_Type Rob_ColorGetRGB(const Rob_ColorSensor_Type *Sensor_cpst,
                               uint8 *Red_pu8, uint8 *Green_pu8, uint8 *Blue_pu8);

/* Sound Sensor Functions */
Rob_Status_Type Rob_SoundInit(Rob_SoundSensor_Type *Sensor_pst,
                             uint16 Pin_u16, float32 Threshold_f32);

Rob_Status_Type Rob_SoundRead(Rob_SoundSensor_Type *Sensor_pst,
                             uint16 AdcValue_u16);

boolean Rob_SoundIsTriggered_b(const Rob_SoundSensor_Type *Sensor_cpst);

/* IMU Sensor Functions */
Rob_Status_Type Rob_ImuInit(Rob_ImuSensor_Type *Sensor_pst, uint8 I2cAddress_u8);

Rob_Status_Type Rob_ImuRead(Rob_ImuSensor_Type *Sensor_pst);

Rob_Status_Type Rob_ImuGetOrientation(const Rob_ImuSensor_Type *Sensor_cpst,
                                     float32 *Roll_pf32, float32 *Pitch_pf32,
                                     float32 *Yaw_pf32);

/* Filtering Functions (using EASML) */
Rob_Status_Type Rob_SensorFilterInit(Rob_SensorFilter_Type *Filter_pst,
                                     float32 TimeConstant_f32,
                                     float32 SampleTime_f32);

Rob_Status_Type Rob_SensorFilterUpdate(Rob_SensorFilter_Type *Filter_pst,
                                      float32 Input_f32, float32 *Output_pf32);

/* System Update Functions */
Rob_Status_Type Rob_SensorSystemUpdate(Rob_SensorSystem_Type *System_pst,
                                      uint32 CurrentTime_u32);

/* Event Handling */
Rob_Status_Type Rob_SensorCheckEvents(const Rob_SensorSystem_Type *System_cpst,
                                     Rob_SensorEvent_Type *Event_pst);

/* Utility Functions */
float32 Rob_SensorAdcToVoltage_f32(uint16 AdcValue_u16, uint16 AdcMax_u16,
                                  float32 VRef_f32);

float32 Rob_SensorDistanceFromVoltage_f32(float32 Voltage_f32);

#endif /* __ROB_SENSOR_H__ */
